function particle= predict(particle, ntr_param, Q)
%
xv= particle.xv;
Pv= particle.Pv;

%[ntr_param]= add_control_noise(tr_param,Q, 1);

% Jacobians     
yy = (ntr_param(4)); % roll (x)
pp = (ntr_param(5)); % pitch (y)
rr = (ntr_param(6)); % yaw (z)

% rr = pi_to_pi(xv(6) + ntr_param(6)); % roll (x)
% pp = pi_to_pi(xv(5) + ntr_param(5)); % pitch (y)
% yy = pi_to_pi(xv(4) + ntr_param(4)); % yaw (z)


xu = ntr_param(1);
yu = ntr_param(2);
zu = ntr_param(3);
sy = sin(yy); cy = cos(yy); % yaw
sp = sin(pp); cp = cos(pp); % pitch
sr = sin(rr); cr = cos(rr); % roll

% predefined matrix elements
f14 = -sy*cp*xu + (-sy*sp*sr-cy*cr)*yu + (-sy*sp*cr+cy*sr)*zu;
f15 = -cy*sp*xu + cy*cp*sr*yu + cy*cp*cr*zu;
f16 = (cy*sp*cr + sy*sr)*yu + (-cy*sp*sr+sy*cr)*zu;
f24 = cy*cp*xu + (cy*sp*sr-sy*cr)*yu + (cy*sp*cr+sy*sr)*zu;
f25 = -sy*sp*xu + sy*cp*sr*yu + sy*cp*cr*zu;
f26 = (sy*sp*cr-cy*sr)*yu + (-sy*sp*sr-cy*cr)*zu;
f35 = -cp*xu-sp*sr*yu - sp*cr*zu;
f36 = cp*cr*yu - cp*sr*zu;

Gv = [1 0 0 f14 f15 f16;
      0 1 0 f24 f25 f26;
      0 0 1   0 f35 f36;
      0 0 0   1   0   0;
      0 0 0   0   1   0;
      0 0 0   0   0   1];
  
Gu = [cy*cp    cy*sp*sr-sy*cr     cy*sp*cr+sy*sr 0 0 0;
      sy*cp    sy*sp*sr+cy*cr     sy*sp*cr-cy*sr 0 0 0;
      -sp      cp*sr              cp*cr          0 0 0;
      0        0                  0              1 0 0;
      0        0                  0              0 1 0;
      0        0                  0              0 0 1];

% predict covariance
particle.Pv= Gv*Pv*Gv' + Gu*Q*Gu';

% predict state
particle.xv = [xv(1) + ntr_param(1);
               xv(2) + ntr_param(2);
               xv(3) + ntr_param(3);
               pi_to_pi(xv(4) + ntr_param(4));
               pi_to_pi(xv(5) + ntr_param(5));
               pi_to_pi(xv(6) + ntr_param(6))];